#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include "turtlesim/msg/pose.hpp"
#include "chapt4_interfaces/srv/partol.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"

using Partol= chapt4_interfaces::srv::Partol;//消息接口名字重定义
using SetParametersResult= rcl_interfaces::msg::SetParametersResult;

class TurtleController: public rclcpp::Node
{
public:
    TurtleController() :Node("turtle_controller")
    {
        this->declare_parameter("k",1.0);//定义从参数，初始值为1.0
        this->declare_parameter("max_speed",1.0);
        this->get_parameter("k",k_);
        this->get_parameter("max_speed",max_speed_);
        this->set_parameter(rclcpp::Parameter("k",2.0));//自身改变自身参数的值

        parameter_callback_handle_ = this->add_on_set_parameters_callback([&](const std::vector<rclcpp::Parameter> & parameters)->rcl_interfaces::msg::SetParametersResult{ //改变参数的值回调函数
            rcl_interfaces::msg::SetParametersResult result;
            result.successful = true;
            for (const auto & parameter : parameters) {
                RCLCPP_INFO(this->get_logger(),"更新参数的值%s=%f",parameter.get_name().c_str(),parameter.as_double());
                if(parameter.get_name()=="k")
                {
                    k_ = parameter.as_double();
                }
                if(parameter.get_name()=="max_speed")
                {
                    max_speed_ = parameter.as_double();
                }
            }
            return result;
            });
        partol_service_ =this->create_service<Partol>("partol",[&](const Partol::Request::SharedPtr request,Partol::Response::SharedPtr response ) ->void{
            if((0<request->target_x && request->target_x<12.0f) &&
                (0<request->target_y && request->target_y<12.0f))
            {
                this->target_x_= request->target_x;
                this->target_y_= request->target_y;
                response->result = Partol::Response::SUCESS;
            }else{
                response->result = Partol::Response::FAIL;

            }
            //额外调用我们添加的回调函数，这样做到第一时间更新参数
        });
        volocity_publisher_=this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel",10);
        pose_subscriber_  =  this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose", 10, std::bind(&TurtleController::on_pose_received_,this,std::placeholders::_1));
    }

private:
    void on_pose_received_(const turtlesim::msg::Pose::SharedPtr pose)//参数：收到数据的共享指针
    {
        auto message = geometry_msgs::msg::Twist();
        //1.获取当前的位置
        auto current_x_=pose->x;
        auto current_y_=pose->y;
        RCLCPP_INFO(this->get_logger(),"当前x=%f,y=%f",current_x_,current_y_);

        //2.计算当前海龟位置和目标位置的距离差和角度差
        auto distance = std::sqrt(
            (target_x_-current_x_)*(target_x_-current_x_)+
            (target_y_-current_y_)*(target_y_-current_y_)
        );
        auto angle = std::atan2((target_y_-current_y_),(target_x_-current_x_)) - pose->theta;

        //3.控制策略
        if(distance>0.1){
            if(fabs(angle)>0.2){
                message.angular.z = fabs(angle);
            }
            else{
                message.linear.x = k_*distance;
            }
        }

        //4.限制线速度最大值
        if(message.linear.x > max_speed_){
            message.linear.x = max_speed_;
        }

        volocity_publisher_->publish(message);
    }

private:
    OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
    rclcpp::Service<Partol>::SharedPtr partol_service_;
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_subscriber_;//订阅者的智能指针
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr volocity_publisher_;//发布者的智能指针
    double target_x_{1.0};
    double target_y_{1.0};
    double k_{1.0};//比例系数
    double max_speed_{3.0};//最大速度
};


int main(int argc,char **argv)
{
    rclcpp::init(argc,argv);
    auto node = std::make_shared<TurtleController>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}